#include <fstream>

#include "featurematcher.h"

#include "dlib/array2d.h"
#include "dlib/image_keypoint.h"
#include "dlib/image_saver/image_saver.h"

#include "math/defines.h"

/* -------------------------------------------------------------- */

FeatureMatcher::FeatureMatcher() 
{
    
}

/* -------------------------------------------------------------- */

FeatureMatcher::~FeatureMatcher()
{
    
}

/* -------------------------------------------------------------- */

float surf_distance_euklid(dlib::surf_point p1, dlib::surf_point p2)
{
	float dist = 0;
	for(std::size_t i = 0 ; i < 64 ; ++i)
	{
		dist += MATH_POW2(p1.des(i,0) - p2.des(i,0));
	}
	return std::sqrt(dist);
}

/* -------------------------------------------------------------- */

std::vector<dlib::surf_point> get_surf_points(mve::ByteImage::Ptr img)
{
	dlib::array2d_kernel_1<dlib::rgb_pixel> dimg;
    dimg.set_size(img->height(), img->width());

	// std::cout << dimg.size() << std::endl;
    
    for (unsigned int i = 0; i < img->width()* img->height(); ++i) 
    {	
		dlib::rgb_pixel pp ( img->at(i,0),
			        		 img->at(i,1),
							 img->at(i,2));
		// std::cout << " \r pixel " << i << "   " << std::flush;
		dimg[i/img->width()][i%img->width()] = pp;
    }
	// std::ofstream os("test2.bmp");
	// dlib::save_bmp(dimg,os);
	// os.close();
	std::vector<dlib::surf_point> sp = dlib::get_surf_points(dimg, 500);
	return sp;
}


/* -------------------------------------------------------------- */

std::vector<FeatureMatch> FeatureMatcher::match_features_surf()
{
    std::cout << "matching surf features ... " << std::endl;

	std::vector<FeatureMatch> matches;
    std::vector<dlib::surf_point> ipoints1, ipoints2;
    
    ipoints1 = get_surf_points(this->img1);
    ipoints2 = get_surf_points(this->img2);

	for(std::size_t i = 0 ; i < ipoints1.size() ; ++i)
	{	
		float min1(999), min2(999);
		std::size_t idx1(0), idx2(0);
		for(std::size_t j = 0 ; j < ipoints2.size() ; ++j)
		{
			float dist = surf_distance_euklid(ipoints1[i], ipoints2[j]);
			if (dist < min1)
			{	
				min2 = min1;
				idx2 = idx1;
				min1 = dist;
				idx1 = j;
			} else if(dist < min2)
			{
				min2 = dist;
				idx2 = j;
			}
		}
		if (min1/min2 < 0.6f)
		{
			math::Vec2f c1(ipoints1[i].p.center(0),ipoints1[i].p.center(1));
			math::Vec2f c2(ipoints2[idx1].p.center(0),ipoints2[idx1].p.center(1));
			// std::cout << "found match " << i << " " << idx1 << std::endl;
			// std::cout << " " << c1 << " xxx " << c2 << std::endl;
			// std::cout << "  " << ipoints1[i].p.scale << "   " << ipoints2[i].p.scale << std::endl;
			matches.push_back(std::make_pair<math::Vec2f, math::Vec2f>(c1, c2));
		}
	}
	return matches;
}

/* -------------------------------------------------------------- */

void FeatureMatcher::get_features(std::vector<math::Vec2f>& p, std::vector<float>& scales)
{
	std::vector<dlib::surf_point> ipoints = get_surf_points(this->img1);
	math::Vec2f center(this->img1->width(), this->img2->height());
	p.clear();
	scales.clear();
	for(std::size_t i = 0; i < ipoints.size(); ++i)
	{	
		math::Vec2f fp(ipoints[i].p.center(0),ipoints[i].p.center(1));
		p.push_back(fp);
		scales.push_back(ipoints[i].p.scale*1.5f);
	}
}

/* -------------------------------------------------------------- */